
import peasy.*;

PeasyCam cam;

float[][] m1;
float[][] m2;
float[][] mResult;

float[] cameraPos;

private float[] angleAxis = new float[] {0,0,0,0,0,0};


private int activeAxis = 0;

boolean wireFrame = false;

void setup(){
  
  size(1280,1024, P3D);
  
  cam = new PeasyCam(this, 500);
  //cam.rotateZ(HALF_PI);
  //cam.rotateY(-HALF_PI);
  cam.rotateX(-HALF_PI);
  cam.pan(0, -100); 
  cam.setMinimumDistance(50);
  cam.setMaximumDistance(5000);
  
  cameraPos = new float[] {0,200,0};
  //camera(cameraPos[0],cameraPos[1],cameraPos[2], 0,0,0, 0,0,-1);
  /*
  m1 = new float[4][4];
  m2 = new float[4][4];
  mResult = new float[4][4];
  
  m1 = Math.Pos_Rot2m(new float[]{0.0,0.0,50.0, 0.0,0.0,0.0});
  m2 = Math.Pos_Rot2m(new float[]{0.0,0.0,1000.0, 0.0,0.0,0.0});
  
  mResult = Math.Tform_mult(m1,m2);
  
  
  printMathMatrix(m1);
  printMathMatrix(m2);
  printMathMatrix(mResult);
  */
}

void draw(){
  background(200);
  
  drawFrame(100);
  
  fill(180);
  
  translate(0,0,3);
  box(100, 100, 6);
  
  translate(0,0,3+25);
  box(20, 20, 50);
  
  translate(0, 0, 25);
  //rotateZ(HALF_PI);
  //rotateX(-HALF_PI);
  //rotateZ(-HALF_PI+angleAxis1);
  
  rotateZ(-HALF_PI + angleAxis[0]);
  noFill();
  //Draw axis
  drawFrame(30);
  if(!wireFrame) fill(20);
  noStroke();
  sphere(20);
  stroke(0);
  translate(0,0,25);
  if(!wireFrame) fill(180);
  box(20, 20, 50);
    
  
  translate(0,0,25);
  if(!wireFrame) fill(20);
  noStroke();
  sphere(20);
  stroke(0);
  
  rotateZ(HALF_PI);
  rotateX(-HALF_PI);
  rotateZ(-HALF_PI + angleAxis[1]);
  
  //Draw axis
  drawFrame(30);
  translate(70,0,0);
  if(!wireFrame) fill(180);
  box(140, 20, 20);
  
  translate(70,0,0);
  if(!wireFrame) fill(20);
  noStroke();
  sphere(20);
  stroke(0);
  
  rotateZ(HALF_PI + angleAxis[2]);
  
  //Draw axis
  drawFrame(30);
  translate(65,0,0);
  if(!wireFrame) fill(180);
  box(130, 20, 20);

  translate(65,0,0);
  if(!wireFrame) fill(20);
  noStroke();
  sphere(20);
  stroke(0);
  
  rotateZ(angleAxis[3]);
  
  //Draw axis
  drawFrame(30);
  translate(25,0,0);
  if(!wireFrame) fill(180);
  box(50, 20, 20);
  
  translate(25,0,0);
  if(!wireFrame) fill(20);
  noStroke();
  sphere(20);
  stroke(0);
  
  
  rotateZ(-HALF_PI);
  rotateX(-HALF_PI);
  
  rotateZ(angleAxis[4]);
  
  drawFrame(30);
  
  translate(0,0,50);
  
  rotateZ(PI);
  drawFrame(50);
  
  resetMatrix();
  
  /*
  stroke(255,0,0);
  line(0,0,0,100,0,0);
  
  stroke(0,255,0);
  line(0,0,0,0,100,0);
  
  stroke(0,0,255);
  line(0,0,0,0,0,100);
  
  stroke(0);
  line(0,0,0,0,0,1500);
  
  pushMatrix(); 
  translate(0,0,3);
  box(100, 100, 6);
  popMatrix();
  
  noStroke();
  
  pushMatrix(); 
  translate(0,0,6+25);
  box(20, 20, 50);
  popMatrix();
  
  pushMatrix(); 
  translate(m1[0][3], m1[1][3], m1[2][3]);
  fill(20);
  sphere(20);
  rotateZ();
  translate(0,0,50);
  fill(180);
  box(20, 20, 100);
  popMatrix();
  
  pushMatrix(); 
  translate(m2[0][3], m2[1][3], m2[2][3]);
  fill(90);
  sphere(30);
  popMatrix();
  
  pushMatrix(); 
  translate(mResult[0][3], mResult[1][3], mResult[2][3]);
  fill(160);
  sphere(40);
  popMatrix();
  */
}

void drawFrame(int sizeOfFrame){
  stroke(255,0,0);
  line(0,0,0,sizeOfFrame,0,0);
  
  stroke(0,255,0);
  line(0,0,0,0,sizeOfFrame,0);
  
  stroke(0,0,255);
  line(0,0,0,0,0,sizeOfFrame);
  
  stroke(0);
}

void printMathMatrix(float[][] m){
  print(str(m[0][0])); print(" ");  print(str(m[0][1])); print(" ");  print(str(m[0][2])); print(" ");  println(str(m[0][3]));
  print(str(m[1][0])); print(" ");  print(str(m[1][1])); print(" ");  print(str(m[1][2])); print(" ");  println(str(m[1][3]));
  print(str(m[2][0])); print(" ");  print(str(m[2][1])); print(" ");  print(str(m[2][2])); print(" ");  println(str(m[2][3]));
  print(str(m[3][0])); print(" ");  print(str(m[3][1])); print(" ");  print(str(m[3][2])); print(" ");  println(str(m[3][3]));
  println("");
}

int getActiveAxis(){
  return activeAxis;
}

void setActiveAxis(int axis){
  activeAxis = axis;
}

void updateAxisValue(float value){
  switch(getActiveAxis()){
    case 1:
      angleAxis[0] += value;
      break;
    case 2:
      angleAxis[1] += value;
      break;
    case 3:
      angleAxis[2] += value;
      break;
    case 4:
      angleAxis[3] += value;
      break;
    case 5:
      angleAxis[4] += value;
      break;
  }  
}

void keyPressed(){
  
  println(keyCode);
  
  switch(keyCode){
    case 37:
      updateAxisValue((PI * 1 / 180.0));
    break;
    case 39:
      updateAxisValue((PI * 1 / 180.0)*-1);
    break;
    case 49:
      setActiveAxis(1);
    break;
    case 50:
      setActiveAxis(2);
    break;
    case 51:
      setActiveAxis(3);
    break;
    case 52:
      setActiveAxis(4);
    break;
    case 53:
      setActiveAxis(5);
    break;
    case 54:
      setActiveAxis(6);
    break;
    case 32:
      wireFrame = wireFrame==false ? true : false;
    break;
  }
  /*
  switch(keyCode){
    case 37:
      //Rotate camera neg. around z-axis 
      float[][] mNewCameraPos = Math.Tform_mult(Math.Rotz2mtr(-1),Math.Pos_Rot2m(new float[]{cameraPos[0],cameraPos[1],cameraPos[2], 0.0,0.0,0.0}));
      
      cameraPos[0] = mNewCameraPos[0][3];
      cameraPos[1] = mNewCameraPos[1][3];
      cameraPos[2] = mNewCameraPos[2][3];
      camera(cameraPos[0],cameraPos[1],cameraPos[2], 0,0,0, 0,0,-1);
      
      break;
    case 38:
      //Rotate camera neg. around z-axis 
      mNewCameraPos = Math.Tform_mult(Math.Roty2mtr(1),Math.Pos_Rot2m(new float[]{cameraPos[0],cameraPos[1],cameraPos[2], 0.0,0.0,0.0}));
      
      cameraPos[0] = mNewCameraPos[0][3];
      cameraPos[1] = mNewCameraPos[1][3];
      cameraPos[2] = mNewCameraPos[2][3];
      camera(cameraPos[0],cameraPos[1],cameraPos[2], 0,0,0, 0,0,-1);
      
      break;
    case 39:
      //Rotate camera pos. around z-axis 
      mNewCameraPos = Math.Tform_mult(Math.Rotz2mtr(1),Math.Pos_Rot2m(new float[]{cameraPos[0],cameraPos[1],cameraPos[2], 0.0,0.0,0.0}));
      
      cameraPos[0] = mNewCameraPos[0][3];
      cameraPos[1] = mNewCameraPos[1][3];
      cameraPos[2] = mNewCameraPos[2][3];
      camera(cameraPos[0],cameraPos[1],cameraPos[2], 0,0,0, 0,0,-1);
      
      break;
    case 40:
      //Rotate camera neg. around z-axis 
      mNewCameraPos = Math.Tform_mult(Math.Roty2mtr(-1),Math.Pos_Rot2m(new float[]{cameraPos[0],cameraPos[1],cameraPos[2], 0.0,0.0,0.0}));
      
      cameraPos[0] = mNewCameraPos[0][3];
      cameraPos[1] = mNewCameraPos[1][3];
      cameraPos[2] = mNewCameraPos[2][3];
      camera(cameraPos[0],cameraPos[1],cameraPos[2], 0,0,0, 0,0,-1);
      
      break;
  }  
  */
}
